#include "userdef.h"
#include "fw_hal.h"
#define TIMER1_FRE (__CONF_FOSC/12)

void dyna_speed(uint16_t fre)
{
	uint16_t dat;
  uint32_t value = TIMER1_FRE/fre;
	dat =0xFFFF>value?(0xFFFF-value):0;
	TH1 =dat >> 8;
	TL1 =dat & 0xFF;
}

INTERRUPT(Timer1_Routine, EXTI_VectTimer1)
{
	MOTOR_STP=!MOTOR_STP;
}

void motor_default(void)
{
	motor_control.dir =DIR_CW;
  motor_control.step_spmax =0;
  motor_control.step_spmix =0;	
	motor_control.step_accel =0;
	motor_control.step_speed =0;
	motor_control.step_state =STA_IDLE;
	motor_control.ntick =0;
	motor_control.wtick =0;
	motor_control.accel_ticks =0;
}

void motor_init(void)
{
	motor_default();
	TIM_Timer1_Config(HAL_State_OFF, TIM_TimerMode_16BitAuto, 1000);
	EXTI_Timer1_SetIntState(HAL_State_ON);
	EXTI_Timer1_SetIntPriority(EXTI_IntPriority_High);
	EXTI_Global_SetIntState(HAL_State_ON);
	TIM_Timer1_SetRunState(HAL_State_OFF);
	MOTOR_EN =DISMOTOR;
}

void motor_moveral(uint8_t dir,uint32_t sec,uint32_t accel,uint32_t speed)
{
	EA =0;
	motor_default();
	motor_control.wtick =SYS_TICK*sec;
	motor_control.step_accel =accel*2;
	motor_control.step_spmax =speed*2;
	motor_control.step_state =STA_ACC;
	MOTOR_DIR =dir;
	MOTOR_EN =ENABMOTOR;
	TIM_Timer1_SetRunState(HAL_State_ON);
	EA =1;
}

void motor_isr(motor_control_t *pmotor)
{
	if(pmotor->step_state ==STA_IDLE)return;

	pmotor->ntick++;
  switch(pmotor->step_state)
	{
		case STA_ACC:
			pmotor->accel_ticks++;
		  pmotor->step_speed=_MIN(pmotor->step_accel*pmotor->ntick/1000+pmotor->step_spmix,pmotor->step_spmax);
		  dyna_speed(pmotor->step_speed);

			if((pmotor->ntick>pmotor->wtick)||(pmotor->step_speed==pmotor->step_spmax)){
				 pmotor->step_state=STA_AVG;
			}
			
			if(pmotor->e_stop==2) pmotor->step_state=STA_DEC;
			
			break;
			
		case STA_AVG:
			if((pmotor->ntick+pmotor->accel_ticks)>pmotor->wtick){
				 pmotor->step_state=STA_DEC;
			}	
			
			if(pmotor->e_stop==2) pmotor->step_state=STA_DEC;
			
			break;
			
		case STA_DEC:
		{
			uint8_t cnt;
			pmotor->step_speed=_MIN(pmotor->step_accel*pmotor->accel_ticks/1000+pmotor->step_spmix,pmotor->step_spmax);
		  dyna_speed(pmotor->step_speed);
			
			cnt =pmotor->e_stop==2?6:1;
			do{if(--pmotor->accel_ticks==0){pmotor->step_state =STA_STOP;break;}}while(--cnt>0);
			break;
		}
		case STA_STOP:
			TIM_Timer1_SetRunState(HAL_State_OFF);
			MOTOR_EN =DISMOTOR;
		  MOTOR_STP =0;
		  pmotor->step_state =STA_IDLE;
		  pmotor->e_stop =0;
			break;
	}
}

void task_mac_clean(uint8_t id,int32_t ntick) reentrant
{
	crBegin(id, ntick);
	motor_moveral(DIR_CW,30,100,500);
	while(motor_control.step_state!=STA_IDLE){crReturn(id);}
	crWait(id,1000);
	motor_moveral(DIR_CCW,30,100,500);
	while(motor_control.step_state!=STA_IDLE){crReturn(id);}
	crWait(id,1000);	

	motor_moveral(DIR_CW,30,4000,30000);
	while(motor_control.step_state!=STA_IDLE){crReturn(id);}
	crWait(id,1000);
	motor_moveral(DIR_CCW,30,4000,30000);
	while(motor_control.step_state!=STA_IDLE){crReturn(id);}
	crWait(id,1000);
	//crDel(id);
  crFinish(id)
}










